clc; clear; close('all');
% Numero de Matricula: M11164
a = 1;
b = 1;
c = 1;
d = 6;
e = 4;
%% Datos del sistema
M = 0.10*(10+e);        % Kg
m = 0.02*(10+d);        % Kg
b = 0.01*(10+c);        % N/(m.s)
l = 0.03*(10+b);        % m
g = 9.81;               % m/s^2
tsim = 15;               % Segundos
t = (0:tsim/100:tsim)';

%% Modelo de estado del sistema linealizado
% Representacion fisica
A = [0      1           0               0
     0      -b/M        -m*g/M          0
     0      0           0               1
     0      b/(M*l)     (m+M)*g/(M*l)   0];
 
B = [0 1/M 0 -1/(M*l)]';
C = [1 0 0 0];
D = 0;
P = eig(A);     % Polos en cadena abierta

%% Numeral 6: Estado final nulo
X0 = [0 0 0.15 0]';     % \theta_0 = 0.15
Q = [1 0 0 0;
     0 0 0 0;
     0 0 1 0;
     0 0 0 0];
R = 1;
[Kr, K, E] = lqr(A,B,Q,R);
Ar = A - B*Kr;
% Plot resultados
[y,x] = lsim(Ar,B,C,D,zeros(length(t),1),t,X0);
figure, plot(t,[x]);
title('LQR Estado Final Nulo. X_0=[0 0 0.15 0]^T','FontWeight','bold');
legend('x', 'xp', '\theta', '\thetap');
xlabel('Tiempo (s)'); grid on;

%% Numeral 7: Mantener \theta por debajo de 0.01 radianes
tsim = 5;               % Segundos
t = (0:tsim/100:tsim)';
Q = [20 0 0 0;
     0 0 0 0;
     0 0 1 0;
     0 0 0 0];
R = 0.1;
[Kr, K, E] = lqr(A,B,Q,R);
Ar = A - B*Kr;
% Plot resultados
[y,x] = lsim(Ar,B,C,D,zeros(length(t),1),t,X0);
u = -Kr*x';
figure, plotyy(t,x, t,u');
title('LQR Estado Final Nulo. |u(t)| < 10N. X_0=[0 0 0.15 0]^T','FontWeight','bold');
legend('x', 'xp', '\theta', '\thetap','u(t)');
xlabel('Tiempo (s)'); grid on;
hold on;
% Lineas en +/-0.01
plot(t,ones(length(t))*0.01,'--k');
plot(t,ones(length(t))*-0.01,'--k');
lineCorte = 0:2.5/100:2.5;

%% Numeral 8: Estado final NO nulo
tsim = 10;               % Segundos
t = (0:tsim/100:tsim)';
X0 = [0 0 0 0]';     
Xf = [0.5 0 0 0]';      % x = 0.5
Q = [1 0 0 0;
     0 0 0 0;
     0 0 1 0;
     0 0 0 0];
R = 1;
[Kr, K, E] = lqr(A,B,Q,R);
Ar = A - B*Kr;
uf = (-pinv(B)*A + Kr) * Xf;
% Plot resultados
[y,x] = lsim(Ar,B,C,D,uf*ones(length(t),1),t,X0);
u = -Kr*x' + uf;
figure, plot(t,[u' x]);
title('LQR. X_0=[0 0 0 0]^T. X_F=[0.5 0 0 0]^T','FontWeight','bold');
legend('u(t)', 'x', 'xp', '\theta', '\thetap');
xlabel('Tiempo (s)'); grid on;
hold on;

%% Numeral 9: Estado final NO nulo
tsim = 8;               % Segundos
t = (0:tsim/100:tsim)';
X0 = [0 0 0 0]';     
Xf = [0.5 0 0 0]';      % x = 0.5
Q = [1 0 0 0;
     0 1 0 0;
     0 0 0 0;
     0 0 0 0];
R = 0.25;
[Kr, K, E] = lqr(A,B,Q,R);
Ar = A - B*Kr;
uf = (-pinv(B)*A + Kr) * Xf;
% Plot resultados
[y,x] = lsim(Ar,B,C,D,uf*ones(length(t),1),t,X0);
u = -Kr*x' + uf;
figure, plot(t,[u' x]);
title('LQR. X_0=[0 0 0 0]^T. X_F=[0.5 0 0 0]^T','FontWeight','bold');
legend('u(t)', 'x', 'xp', '\theta', '\thetap');
xlabel('Tiempo (s)'); grid on;
hold on;
% Linea tiempo establecimiento
plot(t,ones(length(t))*0.5*0.95,'--k');
% Linea sobreoscilacion
plot(t,ones(length(t))*0.5*1.01,'--k');
lineCorte = 0:2.5/100:2.5;